import time
import threading
from datetime import datetime

from . import WD_GPIO

# 按键值定义
from wd_chao.utils import RunUtil

run_car = '1'  # 按键前
back_car = '2'  # 按键后
left_car = '3'  # 按键左
right_car = '4'  # 按键右
stop_car = '0'  # 按键停

# 变量的定义
# 七彩灯RGB三色变量定义
red = 0
green = 0
blue = 0
# 小车和舵机状态变量
g_ServoState = 0
# 小车速度变量
WD_GPIO.CarSpeedControl = 50
# 寻迹，避障，寻光变量
infrared_track_value = ''
infrared_avoid_value = ''
LDR_value = ''
g_lednum = 0

# 指令执行时间
cmd_times = {}


# tcp数据解析并指定相应的动作
def tcp_data_parse(InputString):
    global g_ServoState
    global red
    global green
    global blue
    global g_lednum

    # 执行自定义指令
    if (InputString.startswith("$4WD.CMD.")):
        cmd_str = InputString[9:13]
        if cmd_str == "REST":
            WD_GPIO.init()
        if cmd_str == "STOP":
            WD_GPIO.brake()

    # 解析上位机发来的舵机云台的控制指令并执行舵机旋转
    # 如:$4WD,PTZ,1,180# 舵机转动到180度
    if (InputString.startswith("$4WD,PTZ.")):
        cmd_times["servo"] = datetime.now();
        # 舵机序号
        ptz_index = int(InputString[9: 10])
        # 舵机角度（上或者下）
        ptz_pos = int(InputString[11: 14])

        if ptz_index == 1:
            WD_GPIO.g_frontServoPos = 180 - ptz_pos

        if ptz_index == 2:
            if ptz_pos == 1:
                WD_GPIO.servo_left()
            elif ptz_pos == 2:
                WD_GPIO.servo_right()
            else:
                WD_GPIO.servo_stop()

        if ptz_index == 3:
            if ptz_pos == 1:
                WD_GPIO.servo_up()
            elif ptz_pos == 2:
                WD_GPIO.servo_down()
            else:
                WD_GPIO.servo_stop()

    # 解析上位机发来的七彩探照灯指令并点亮相应的颜色
    # 如:$4WD,CLR255,CLG0,CLB0# 七彩灯亮红色
    if (InputString.find("CLR", 0, len(InputString)) != -1):
        i = InputString.find("CLR", 0, len(InputString))
        ii = InputString.find(",CLG", 0, len(InputString))
        if ii > i:
            string = InputString[i + 3:ii]
            m_kp = int(string)
            red = m_kp
        i = InputString.find("CLG", 0, len(InputString))
        ii = InputString.find(",CLB", 0, len(InputString))
        if ii > i:
            string = InputString[i + 3:ii]
            m_kp = int(string)
            green = m_kp
        i = InputString.find("CLB", 0, len(InputString))
        ii = InputString.find("#", 0, len(InputString))
        if ii > i:
            string = InputString[i + 3:ii]
            m_kp = int(string)
            blue = m_kp
            WD_GPIO.color_led_pwm(red, green, blue)
        InputString.zfill(len(InputString))

    # 小车运动控制
    # 如:$4WD.RUN,1,1#    小车前进
    if (InputString.startswith("$4WD.RUN.")):
        cmd_times["run"] = datetime.now();
        if InputString[11] == '1':
            WD_GPIO.CarSpeedControl += 10
            if WD_GPIO.CarSpeedControl > 100:
                WD_GPIO.CarSpeedControl = 100  # 小车加速
        if InputString[11] == '2':
            WD_GPIO.CarSpeedControl -= 10
            if WD_GPIO.CarSpeedControl < 10:  # 小车减速
                WD_GPIO.CarSpeedControl = 10

        g_car_state = int(InputString[9])
        # 根据解析的数据让小车做出相应的运动
        if g_car_state == WD_GPIO.enSTOP:
            WD_GPIO.brake()
        elif g_car_state == WD_GPIO.enRUN:
            WD_GPIO.run()
        elif g_car_state == WD_GPIO.enLEFT:
            WD_GPIO.left()
        elif g_car_state == WD_GPIO.enRIGHT:
            WD_GPIO.right()
        elif g_car_state == WD_GPIO.enBACK:
            WD_GPIO.back()
        elif g_car_state == WD_GPIO.enTLEFT:
            WD_GPIO.spin_left()
        elif g_car_state == WD_GPIO.enTRIGHT:
            WD_GPIO.spin_right()
        else:
            WD_GPIO.brake()

    # 解析上位机发来的通用协议指令,并执行相应的动作
    # 如:$$4WD,1,0,0,0,0,0,0,0#    小车鸣笛
    if (InputString.startswith("$4WD.ETC")):
        if InputString[9] == '1':
            WD_GPIO.whistle()  # 小车鸣笛
        # 灭火
        if InputString[9] == '2':
            WD_GPIO.out_fire()
            time.sleep(1)

        # 小车点亮指定颜色
        if InputString[13] == '1':
            g_lednum = g_lednum + 1
            if g_lednum == 1:
                WD_GPIO.color_led_pwm(255, 255, 255)
            elif g_lednum == 2:
                WD_GPIO.color_led_pwm(255, 0, 0)
            elif g_lednum == 3:
                WD_GPIO.color_led_pwm(0, 255, 0)
            elif g_lednum == 4:
                WD_GPIO.color_led_pwm(0, 0, 255)
            elif g_lednum == 5:
                WD_GPIO.color_led_pwm(255, 255, 0)
            elif g_lednum == 6:
                WD_GPIO.color_led_pwm(0, 255, 255)
            elif g_lednum == 7:
                WD_GPIO.color_led_pwm(255, 0, 255)
            else:
                WD_GPIO.color_led_pwm(0, 0, 0)
            g_lednum = 0

        if InputString[13] == '2':
            WD_GPIO.color_led_pwm(255, 0, 0)
        if InputString[13] == '3':
            WD_GPIO.color_led_pwm(0, 255, 0)
        if InputString[13] == '4':
            WD_GPIO.color_led_pwm(0, 0, 255)


# 对Tcp传过来的数据封包
def Data_Pack(InputString):
    print("InputString: %s" % InputString)
    if InputString[0] == '$' and InputString.find("#", 0, len(InputString)) != -1:
        # 调用数据解析函数
        tcp_data_parse(InputString)


# 采集的传感器数据串口回发给上位机显示
def data_postback():
    # 小车超声波传感器采集的信息发给上位机显示
    # 打包格式如:
    #    超声波 电压  灰度  巡线  红外避障 寻光
    # $4WD,CSB120,PV8.3,GS214,LF1011,HW11,GM11#
    ReturnTemp = {}
    distance = WD_GPIO.Distance_test()
    ReturnTemp["distance"] = distance

    track_value = WD_GPIO.tracking_test()
    ReturnTemp["tracking"] = track_value

    avoid_value = WD_GPIO.infrared_avoid_test()
    ReturnTemp["avoid_value"] = avoid_value

    LDR_value = WD_GPIO.follow_light_test()
    ReturnTemp["LDR"] = LDR_value

    print("ReturnTemp: %s" % ReturnTemp)
    return ReturnTemp


# 一秒后没有操作，执行停止指令
def auto_stop():
    while True:
        time.sleep(0.1)
        for tag, exe_time in cmd_times.items():
            now_time = datetime.now();
            grap_time = now_time - exe_time
            if grap_time.seconds > 1:
                if tag == "run":
                    WD_GPIO.brake()
                elif tag == "servo":
                    WD_GPIO.servo_stop()


def start_server():
    try:
        # 引脚初始化
        WD_GPIO.init()
        # 舵机归为
        WD_GPIO.servo_init()
        # 启动自动终止线程
        threading.Thread(target=auto_stop).start()

    except KeyboardInterrupt:
        print(KeyboardInterrupt)
        pass
